#include  "RoboDevice.h"
#include  "RoboDrive.h"
#include "vex.h"
#include "RoboClamp.h"
using namespace vex;
namespace RoboClamp{

    void RoboClampHead_Lock_1(){    //自动阶段锁
        while (RoboDevice::RoboClampHeadUp.position(deg)<101)
        {   
            RoboDevice::RoboClampHeadUp.spin(fwd,150,rpm);
        }
        RoboDevice::RoboClampHeadUp.stop(hold);
        while (RoboDevice::RoboClampHeadDown.position(deg)<25)
        {   
            RoboDevice::RoboClampHeadDown.spin(fwd,100,rpm);
        }
        RoboDevice::RoboClampHeadDown.stop(hold); 
    }

    void RoboClampHead_Release_1(){    //自动阶段松
        while (RoboDevice::RoboClampHeadUp.position(deg)>0)
        {   
            RoboDevice::RoboClampHeadUp.spin(fwd,-75,rpm);
        }
        RoboDevice::RoboClampHeadUp.stop(brake);
        while (RoboDevice::RoboClampHeadDown.position(deg)>0)
        {   
            RoboDevice::RoboClampHeadDown.spin(fwd,-50,rpm);
        }
        RoboDevice::RoboClampHeadDown.stop(brake); 
    }
    void RoboClampHead_Release(){    //手动阶段松

        while(1){
          RoboDevice::RoboClampHeadUp.spin(fwd,-1.0*(RoboDevice::RoboClampHeadUp.position(deg)),rpm);
          RoboDevice::RoboClampHeadDown.spin(fwd,-1.0*(RoboDevice::RoboClampHeadDown.position(deg)),rpm);
          wait(0.02,sec);
        }
        
    }
    void RoboClampHead_Lock(){    //手动阶段锁

        while(1){
        RoboDevice::RoboClampHeadUp.spin(fwd,1.0*(120.0-RoboDevice::RoboClampHeadUp.position(deg)),rpm);
        RoboDevice::RoboClampHeadDown.spin(fwd,1.0*(60.0-RoboDevice::RoboClampHeadDown.position(deg)),rpm);
        wait(0.02,sec);
           
      }
    }

    
    //后钩子
    void RoboClampBack_Release(){    //松

        while(1){
          RoboDevice::RoboClampBack.spin(fwd,0.54*(-20-RoboDevice::RoboClampBack.position(deg)),rpm);
          wait(0.02,sec);
        }
        
    }

    void RoboClampBack_Lock(){    //锁

        for(int i = 0; i <17;i++){
        RoboDevice::RoboClampBack.spin(fwd,1.0/2.0*(60-RoboDevice::RoboClampBack.position(deg)),rpm);
        wait(0.02,sec);
        }    
    }
    //************************************************************************
    void RoboClampHead_Release_One(){    //松

        for(int i = 1;i<= 30;i++){
          RoboDevice::RoboClampHeadUp.spin(fwd,-1.0*(RoboDevice::RoboClampHeadUp.position(deg)),rpm);
          RoboDevice::RoboClampHeadDown.spin(fwd,-1.0*(RoboDevice::RoboClampHeadDown.position(deg)),rpm);
          wait(0.02,sec);
        }
        
    }//松

    void RoboClampHead_Lock_One(){    //锁

        for(int i = 1;i<= 30;i++){
        RoboDevice::RoboClampHeadUp.spin(fwd,1.0*(120.0-RoboDevice::RoboClampHeadUp.position(deg)),rpm);
        RoboDevice::RoboClampHeadDown.spin(fwd,1.0*(60.0-RoboDevice::RoboClampHeadDown.position(deg)),rpm);
        wait(0.02,sec);
        }    
    }
    
    //后钩子
    void RoboClampBack_Release_One(){    //松

        for(int i = 1;i<= 30;i++){
          RoboDevice::RoboClampBack.spin(fwd,0.54*(-20-RoboDevice::RoboClampBack.position(deg)),rpm);
          wait(0.02,sec);
        }
        
    }

    void RoboClampBack_Lock_One(){    //锁

        for(int i = 1;i<= 30;i++){
        RoboDevice::RoboClampBack.spin(fwd,1.0/2.0*(60-RoboDevice::RoboClampBack.position(deg)),rpm);
        wait(0.02,sec);
        }    
    }
}